#include <vector>

#include "app.hpp"
#include "cv_lidar.hpp"
#include "lidar.hpp"
#include "log.hpp"
#include "spdlog/spdlog.h"

class LidarAllDemo : public App {
 private:
  Lidar lidar_;
  LidarField caller_;

 public:
  LidarAllDemo(const std::string &log_path) : App(log_path) {
    lidar_.Open();
    caller_.SetDrivePoint(component::LidarPoint(0, 450));
    SPDLOG_WARN("************ Starting *************");
  }

  ~LidarAllDemo() { SPDLOG_WARN("************ End Running *************"); }

  void Run() {
    SPDLOG_WARN("************ Running *************");

    std::vector<component::LidarPoint> points;

    while (true) {
      points.clear();
      points = lidar_.GetData();
      if (points.size() > 0) {
        SPDLOG_INFO("GetData Success");
        caller_.SetObstracts(points);
        caller_.CallDirection(500);

        cv::Mat mat = caller_.VisualizeObstracts(6);
        cv::imshow("image", mat);
        cv::waitKey(10);
      }
    }
  }
};

int main() {
  LidarAllDemo demo("logs/log.log");
  demo.Run();
  return EXIT_SUCCESS;
}